"""my_new_controller_1114 controller."""

import numpy as np
import math

from controller import Robot, Motor, PositionSensor, InertialUnit, Accelerometer
# from mypackage import Discreteness, PID_control, WLRobot, Leg, KalmanLinear, lagrange_dof8

np.set_printoptions(precision=4, suppress=True, linewidth=300)

robot = Robot()

timestep = int(robot.getBasicTimeStep())

# 足电机
motor_l = Motor('motor5')
motor_r = Motor('motor6')

# 髋电机
motor_1 = Motor('motor1')
motor_3 = Motor('motor3')

motor_2 = Motor('motor2')
motor_4 = Motor('motor4')

ps_1 = PositionSensor('ps1')
ps_3 = PositionSensor('ps3')

ps_2 = PositionSensor('ps2')
ps_4 = PositionSensor('ps4')


ps_l = PositionSensor('ps5')
ps_r = PositionSensor('ps6')
imu = InertialUnit('imu')

accelerometer = Accelerometer('accelerometer')

for motor in [motor_l,motor_r]:
    motor.setPosition(float('inf'))
    motor.setVelocity(0)
    #
    # motor.setPosition(0)
    # motor.setVelocity(float('inf'))
    # motor.setControlPID(100,0,0)
#
for motor in [motor_1, motor_2,motor_3, motor_4]:
    # motor.setPosition(float('inf'))
    # motor.setVelocity(0)

    motor.setPosition(0)
    motor.setVelocity(float('inf'))
    motor.setControlPID(100,0,0)
# for motor in [motor_l,motor_r]:
#     motor.setPosition(0)
#     motor.setVelocity(float('inf'))

for sensor in [ps_1, ps_2,ps_3, ps_4, imu, accelerometer, ps_l, ps_r]:
    sensor.enable(timestep)


d_t = timestep / 1000
sim_time = 0
while robot.step(timestep) != -1:
    sim_time = sim_time + d_t
    # theta_3 = -0.3
    # theta_4 = 0.3
    theta_1= -0.3
    theta_3= -0.3
    theta_2= 0.3
    theta_4= 0.3
    # theta_3 = -0.6
    motor_1.setPosition(theta_1)
    motor_2.setPosition(theta_2)
    motor_3.setPosition(theta_3)
    motor_4.setPosition(theta_4)
    motor_l.setForce(0.0001)
    motor_r.setForce(0.0001)